////////////////////////////////////////////////////////////////////////////////
// SimuCab 2.0 car simulation
// Author SEDRUE JEAN-DAVID
// http://imaginesite.fr
// imaginesiteweb@gmail.com
//
// open source code, respect gpl license terms
// permission to copy,distribute and/or modify it.
// If you want to use it, please quote author and source. 
//
// thx to RacingMat for support and bug correction
////////////////////////////////////////////////////////////////////////////////
// sketchbook 0.65, arduino uno
// motor driver shield : POLULU VNH5019
// led display for game gear,speed and rpm TM1638
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// MOTOR DEFINITIONS & DECLARATIONS
////////////////////////////////////////////////////////////////////////////////
#define motLeft 1
#define motRigth 2
#define potR A4
#define potL A5
#define initialPosition 500
#include "DualVNH5019MotorShield.h"
#include <TM1638.h>
DualVNH5019MotorShield md;
int DataValueL; //middle position 
int DataValueR; //middle position 
int sensorL,sensorR;
int pwmUp=400;//400 MAXneed Power to go up !
int pwmDown=-100;
int pwmLeftMotor=0;
int pwmRigthMotor=0;
int Tol=20; //10 NORMALLY
int difference;
int pwm;
int securityLow=300;//300;
int securityHigh=700;//800;
int brakeDistance=50;
int rpmCoef=1;
byte buttons;
////////////////////////////////////////////////////////////////////////////////
// TACHOMETER DEFINITIONS & DECLARATIONS
////////////////////////////////////////////////////////////////////////////////
TM1638 module(3, 5, 11);
int DataValueS=255; //speed
int DataValueG=0; //gear
int DataValueF=0; //rpm
String Gear;
String theSpeed;
////////////////////////////////////////////////////////////////////////////////
//  INIT PROCEDURE
////////////////////////////////////////////////////////////////////////////////
void setup()
{
  /* serial initialization */
  Serial.begin(115200);
  /* motor initialization */
  md.init();
  DataValueL=initialPosition; //middle position 
  DataValueR=initialPosition; //middle position 
  /*init lcd*/
  setupDisplay();
   
}
void setupDisplay()
{ 
  //module.setDisplayToHexNumber(0x1234ABCD, 0xF0); 
  module.setupDisplay(true, 7);
  module.clearDisplay();
}
////////////////////////////////////////////////////////////////////////////////
///////////////////////////////// Main Loop ////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
//M2 rigth motor
void loop(){
  serialData();
  //speed  
  theSpeed=setTheSpeed((DataValueS-256));
  module.setDisplayToString(theSpeed);
  // Gear
  Gear=CalculateGear(DataValueG);
  module.setDisplayToString(String(Gear),0,7);
  // need to init at each connection
  if (DataValueG==255) {
    setupDisplay();
    DataValueG=0;
  }  
  //rpm
  if (DataValueF<250) SetLedsForRpm(DataValueF);  
  //buton of TM1638
    // button pressed - change mode
  buttons= module.getButtons();
  if (buttons == 1) {
   setupDisplay();
   //Serial.println(buttons);
  }
  if (buttons == 2) rpmCoef=1;
  if (buttons == 4) rpmCoef=2;
  //motion simulator
  sensorR = analogRead(potR);  
  sensorL = analogRead(potL); // echelle 0-1000
  //  sensorsDisplay();
  motorMotion(motRigth,sensorR,DataValueR);
  motorMotion(motLeft,sensorL,DataValueL);  
  
}
////////////////////////////////////////////////////////
void motorMotion(int numMot,int actualPos,int targetPos)
////////////////////////////////////////////////////////
{
  int force=pwmUp;
  boolean moving=true;  
  // encadrements des valeurs
  if (targetPos<securityLow+brakeDistance)   targetPos=(securityLow+brakeDistance);    //software limits, but consideration speed brake.
  if (targetPos>securityHigh-brakeDistance)  targetPos=(securityHigh-brakeDistance); //software limits, but consideration speed brake.
  // pay attention for security for motor!
  if (actualPos>securityHigh || actualPos<securityLow) moving=false; //mechanicals limits
  //to near to move
  difference=abs(actualPos-targetPos);
  if (difference<Tol) moving=false; //to near to move    
  // go up
  if (actualPos<targetPos && moving) {
    force=pwmUp;
    if (difference<150) force=350;
    if (difference<100) force=325;
    if (difference<50) force=300;
    if (numMot==1) md.setM1Speed(force);
    if (numMot==2) md.setM2Speed(force);    
  }
  // go down  
  if (actualPos>targetPos && moving){
    if (numMot==1) {
      md.setM1Speed(pwmDown);
    }
    if (numMot==2) {
      md.setM2Speed(pwmDown);
    }
  }
  // do not move
  if (moving==false)motorOff(numMot);
}
////////////////////////////////////////////////////////////////////////////////
void motorOff(int motor){
  ////////////////////////////////////////////////////////////////////////////////
  if (motor==1) md.setM1Speed(0); //left
  if (motor==2) md.setM2Speed(0); //rigth
}
////////////////////////////////////////////////////////////////////////////////
void sensorsDisplay(){
  Serial.print("L:");
  Serial.print(sensorL);
  Serial.print(" R:");
  Serial.println(sensorR);
}
////////////////////////////////////////////////////////////////////////////////
// complete speed length for always 3 chars
////////////////////////////////////////////////////////////////////////////////
String setTheSpeed(int x)
{
  String y=String(x);
  if (y.length()==1) return ("00"+y);
  if (y.length()==2) return ("0"+y);
  return y;
}

////////////////////////////////////////////////////////////////////////////////
void SetLedsForRpm(long x)
{
  word leds;
  if (x<120) leds=0b0000000000000000;
  if (x>120) leds=0b0000000100000000;
  if (x>150) leds=0b0000001100000000;
  if (x>160) leds=0b0000011100000000;
  if (x>170) leds=0b0000111100000000;
  if (x>180) leds=0b0001111100000000;
  if (x>190) leds=0b0011111100000000;
  if (x>205) leds=0b0011111101000000;
  if (x>210) leds=0b0011111111000000;
  if (x>215) leds=0b0000000011111111;
  module.setLEDs(leds);

}
////////////////////////////////////////////////////////////////////////////////
String CalculateGear(int x)
{
  if (x==117) return "H";
  if (x==127) return "0";
  if (x==138) return "1";
  if (x==149) return "2";
  if (x==159) return "3";
  if (x==170) return "4";
  if (x==181) return "5";
  if (x==191) return "6";
  return "d";

}
////////////////////////////////////////////////////////////////////////////////
// Procedure: wait for complete trame
////////////////////////////////////////////////////////////////////////////////
void serialData()
{ 
  byte Data[3]={
    '0','0','0'              };
  // il faut limmitter le temps pour garder le contrÃƒÂ´le des moteurs
  if (Serial.available()>2){
    //test if order  R or L data type
    Data[0]=Serial.read();
    if (Data[0]=='R'){ // ordre connue ?, si non je verrais au prochain tour !
      Data[1]=Serial.read();
      Data[2]=Serial.read();
      DataValueR=4*NormalizeData(Data);
    }
    if (Data[0]=='L'){ // ordre connue ?, si non je verrais au prochain tour !
      Data[1]=Serial.read();
      Data[2]=Serial.read();
      DataValueL=4*NormalizeData(Data);
    }
    // speed
    if (Data[0]=='S'){ // ordre connue ?, si non je verrais au prochain tour !
      Data[1]=Serial.read();
      Data[2]=Serial.read();
      DataValueS=2*NormalizeData(Data);//2*word(Data[1],Data[2]);
      //module.clearDisplay(); //need if new speed
    }      
    //gear
    if (Data[0]=='G'){ // ordre connue ?, si non je verrais au prochain tour !
      Data[1]=Serial.read();
      Data[2]=Serial.read();
      DataValueG=NormalizeData(Data);      
    }
    //rotation per minute of motor : Frequency
    if (Data[0]=='T'){ // 
      Data[1]=Serial.read();
      Data[2]=Serial.read();
      if (rpmCoef==1)DataValueF=NormalizeData(Data);
      if (rpmCoef==2)DataValueF=NormalizeData(Data)-39;      
    }
    //INIT TACHO WITH XSIM AFTER POWER DOWN
    if (Data[0]=='I'){ // 
      Data[1]=Serial.read();
      Data[2]=Serial.read();
      //module.setupDisplay(true, 7);      
    }


  }
  //if (Serial.available()>32) Serial.flush();
  if (Serial.available()>16) Serial.flush();
}
////////////////////////////////////////////////////////////////////////////////
// testPot
////////////////////////////////////////////////////////////////////////////////
void testPot(){

  Serial.print(analogRead(A4));
  Serial.print(";");
  Serial.println(analogRead(A5));
  delay(250);

}

////////////////////////////////////////////////////////////////////////////////
// Function: convert Hex to Dec
////////////////////////////////////////////////////////////////////////////////
int NormalizeData(byte x[3])
////////////////////////////////////////////////////////////////////////////////
// Bug Correction by RacingMat on line 273.
////////////////////////////////////////////////////////////////////////////////
{
  int result;

  if ((x[2]==13) || (x[2]=='R') || (x[2]=='L'))  //only a LSB and Carrier Return or 'L' or 'R' in case of value below 16 (ie one CHAR and not 2)
  {
    x[2]=x[1];  //move MSB to LSB
    x[1]='0';     //clear MSB
  }
  for (int i=1; i<3; i++)
  {
    if (x[i]>47 && x[i]<58 ){//for xA to xF
      x[i]=x[i]-48;
    }                      
    if (  x[i]>64 && x[i]<71 ){//for x0 to x9
      x[i]=x[i]-55;               
    }
  }
  result=(x[1]*16+x[2]);
  return result;
}


